from camera import cam
from ATag import AprilTag
from Main.Class.Message import MTX,DIST, MTX_NEW
import numpy as np
import cv2
import copy
from pointsenter import Sender
from PointTF import  PointCalculation
# from cal import CalJoint
from JointCal import MoveTo,Home

def Cal(point,dis):
    u,v = point
    point1 = np.array([u*dis,v*dis,dis])
    inmtx = np.linalg.inv(MTX_NEW)
    point3D = np.matmul(inmtx ,point1)
    return point3D


c = cam()
a = AprilTag()
s = Sender()
tf = PointCalculation()

while 1:
    Home()

    cv2.waitKey(100)

    img = c.getimage()
    returnList,img = a.detect(img)
    # print(returnList)

    if len(returnList)>0:
        for r in returnList:
            print(r)
            distance = c.getdistance([int(r[0]),int(r[1])])
            # distance = 690
            
            imga = copy.copy(img)
            # imgdraw = cv2.putText(imga, str(distance), tuple(r), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
            PointCam = Cal(r,distance)
            PointCam = np.array(PointCam)/1000

            #####################

            pointpanda  = np.array(tf.pointcal(PointCam))
            #  0.55007286,  0.11882157,  0.22947234

            # pointpanda = np.array(pointpanda)
            s.sent(pointpanda)

            MoveTo(pointpanda,MoveFlag=True)
            exit()
            
            # print(CalJoint(pointpanda))

        cv2.imshow("",imga)

    # else:
        cv2.imshow("",img)

    cv2.waitKey(1)
    # exit()